#include <a02_task3/control_cmd.h>
#include <ros/ros.h>
#include <time.h>

#include "ugv_sdk/scout/scout_base.hpp"

using namespace westonrobot;

class CmdController {
 private:
  ros::NodeHandle nh_;
  ros::Subscriber sub_;
  ScoutBase scout_;

 public:
  CmdController() {
    sub_ = nh_.subscribe<a02_task3::control_cmd>(
        "car_control_cmd", 1000, &CmdController::doPose, this);
    scout_.Connect("can0", 0);
    scout_.EnableCommandedMode();
  }

  void doPose(const a02_task3::control_cmd::ConstPtr &p) {
    // from @liujun
    // 没来新消息，一直读队首的消息，队首的消息是上一次的消息，所以一直是上一次的速度
    while (ros::ok) {
      ros::spinOnce();
      scout_.SetMotionCommand(p->linear_velocity, p->angular_velocity);
    }
    ROS_INFO("linear_velocity is %.2f, angular_velocity is %.2f",
             p->linear_velocity, p->angular_velocity);
  }
};

int main(int argc, char *argv[]) {
  ros::init(argc, argv, "control_car_by_remote_command");
  CmdController cmd_contrller;
  ros::spin();
  return 0;
}